#!/usr/bin/python
# -*- coding: UTF-8 -*-
import sys;
sys.path.append("..")
import time

class ManipulatorActions:
    #动作组列表
    actions_groups = []
    #动作列表
    actions = []

    #指示是否正在运行动作列表
    is_running = False

    #指示当前构型是否处理使能状态
    is_motorsEnable = True
    
    def __init__(self,robotlib):
        self.robotlib = robotlib
        self.lib = self.robotlib.mclib


    #新增动作组
    def newGroup(self):
        self.actions = []
        self.actions_groups.append(self.actions)
        self.is_running = False
        pass

    #加载动作组
    def loadGroup(self,num):
        size = len(self.actions_groups)
        if(size < num or num < 1):
            print("不存在序号为" + str(num) + "的动作组")
            return False

        index = num - 1
        print("正在加载" + str(index) + "/" + str(size))
        self.actions = self.actions_groups[index]
        print("动作列表：")
        print(self.actions)
        return True

    #清空当前动作列表
    def clear(self):
        if(self.is_running):
            self.is_running = False
        self.actions = []
        print("清空完成")

    #添加当前姿态到动作列表
    def add(self,action):
        size = len(action)
        adding = [0.0] * size
        for i in range(size):
            adding[i] = action[i]
        self.actions.append(adding)
        print("已新增动作-序号" + str(self.actions))

    #运行动作列表
    def runAction(self,action):
        idsLen = len(self.robotlib.module_ids)
        anglesLen = len(action)
        for i in range(idsLen):
            if(i < anglesLen):
               self.lib.SetMotorPosition(self.robotlib.module_ids[i],action[i])
               time.sleep(0.02)
        pass

    #运行当前动作组
    def run(self):
        if(self.is_running):
            print("请先停止当前运行")
            return
        self.is_running = True
        for a in self.actions:
            if(self.is_running == False):
               print("运行中止")
               break
            print("执行动作" + str(a))
            self.runAction(a)
            time.sleep(2)

        self.is_running = False
        print("运行完成")

    #停止动作
    def stop(self):
        self.is_running = False
        pass


    #设置舵机是否使能
    def enableMotors(self,isEnable):
        idsLen = len(self.robotlib.module_ids)
        self.is_motorsEnable = isEnable
        for i in range(idsLen):
            self.lib.EnableMotor(self.robotlib.module_ids[i],isEnable)
            time.sleep(0.02)
        if isEnable:
          print("已开启舵机使能")
        else:
          print("已关闭舵机使能")

    #读取模块当前位置
    def readMotorsPositions(self,manipulator):
        idsLen = len(self.robotlib.module_ids)
        for i in range(idsLen):
            manipulator.position_angles[i] = self.lib.GetMotorPosition(self.robotlib.module_ids[i])
            time.sleep(0.02)
        print("读取关节角度位置完成")
        return manipulator.position_angles




